
#include "ChangeDetection/include/ChangeDetectionControlArchitecture.h"
#include "ChangeDetection/include/ChangeDetectionAgentWorldModel.h"
#include "World/World.h"
#include "ChangeDetection/include/ChangeDetectionController.h"
#include "Behavior/WallAvoidanceBehavior.h"

//BELOW IS DISABLED BECAUSE OF DISABLING EVOLUTION
/*double ChangeDetectionControlArchitecture::migrationRate;
double ChangeDetectionControlArchitecture::eliteSize;
double ChangeDetectionControlArchitecture::bestRate;
int ChangeDetectionControlArchitecture::migrationType;
int ChangeDetectionControlArchitecture::topologyType;*/

int ChangeDetectionAgentWorldModel::PIXELS_FROM_EDGE_RESET = 50;

ChangeDetectionControlArchitecture::ChangeDetectionControlArchitecture(RobotAgentWorldModel* __wm) : DifferentialControlArchitecture(__wm) {
	//setPopulation(new HiveMindInstance());
}

ChangeDetectionControlArchitecture::~ChangeDetectionControlArchitecture() {
	// nothing to do.
}

void ChangeDetectionControlArchitecture::createRandomGenome() { //used to be ControllerPtr instead of void, returning the ControllerPtr object that is now global
	/*
	 * Inputs:
	 * Distance sensors
	 * Ditch sensors
	 * organism size
	 * angle to goal
	 * distance to goal
	 * bias node
	 * 
	 * Outputs:
	 * 5x vote for a strategy
	 * organism size
	 * direction
	 * speed
	 *
	 * interesting for evolution, but not now
	 */
/*	int nbWeights = ((_wm->getDefaultSensors()->getSensorCount() * 2) + 4) * 8;
	vector<double> weights = vector<double>(nbWeights, 0);
	for (int i = 0; i < nbWeights; i++) {
		weights[i] = Rand::randouble() * 2.0 - 1;
	}*/

	//ControllerPtr p = boost::make_shared<ChangeDetectionController>();
	//double left = 1, right = 1;
	//p->step(left, right);
	j = boost::make_shared<ChangeDetectionController>();
	//ControllerPtr q = boost::make_shared<ChangeDetectionController>(weights);
    //return q;
}


void ChangeDetectionControlArchitecture::step() {
	//ControllerPtr j = this->
	//init controller if there is none yet
	//WallAvoidanceBehavior::step(_wm);
	if(j.get()==NULL){
		//cerr << "[O YEAH] controller initialized" << endl;
		createRandomGenome();
		j ->setWorldModel(_wm);
	}
	double left =0, right =0;
	j->step(left,right);
	this->setSpeed(left,right);

	/*if(gWorld->getIterations()%1 == 0){
		calculateStepFitness();
		j->reset();
	}*/

	//j->reset();
}

//for future use, needed as there is no evolution (this method is otherwise implemented in the evolution part)
void ChangeDetectionControlArchitecture::reset()
{
	// RESET AGENT HERE (candidate.reset())!!!!
	j.reset();
}

double ChangeDetectionControlArchitecture::calculateStepFitness() {
	ChangeDetectionAgentWorldModel* worldModel = dynamic_cast<ChangeDetectionAgentWorldModel*> (_wm);

	double distance = worldModel->distanceTraveledLastIteration;
	double fitness = distance;
	std::cout << fitness << std::endl;

	return fitness;
}

//old method, currently out of use as evolution is not used
/*double ChangeDetectionControlArchitecture::calculateStepFitness() {
	ChangeDetectionAgentWorldModel* worldModel = dynamic_cast<ChangeDetectionAgentWorldModel*> (_wm);
	double distance;
	if(worldModel->wrapAround==false){
		distance = worldModel->getPosition().x - worldModel->xStart;
	}else{
		distance = (gAreaWidth-worldModel->xStart-worldModel->PIXELS_FROM_EDGE_RESET)//distance from start to wraparound point
				+(worldModel->getPosition().x);//distance traveled after wraparound
	}
	//distance = worldModel->getPosition().x - worldModel->xStart;
	double fitness = distance;
	return fitness;
	return 1;
}*/

//old method, now out of use (might be useful in future)
/*ControllerPtr ChangeDetectionControlArchitecture::createRandomGenome() {

	 * Inputs:
	 * Distance sensors
	 * Ditch sensors
	 * organism size
	 * angle to goal
	 * distance to goal
	 * bias node
	 *
	 * Outputs:
	 * 5x vote for a strategy
	 * organism size
	 * direction
	 * speed

	int nbWeights = ((_wm->getDefaultSensors()->getSensorCount() * 2) + 4) * 8;
	vector<double> weights = vector<double>(nbWeights, 0);
	for (int i = 0; i < nbWeights; i++) {
		weights[i] = Rand::randouble() * 2.0 - 1;
	}

	ControllerPtr q = boost::make_shared<ChangeDetectionController>(weights);
    return q;
}*/
